#!/usr/bin/env python3

from launch import LaunchDescription
from launch_ros.actions import Node 
from launch.actions import DeclareLaunchArgument, ExecuteProcess, GroupAction
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition, UnlessCondition

def generate_launch_description():
    use_python_arg = DeclareLaunchArgument(
        "use_python",
        default_value="False"
    )

    wheel_radius_arg = DeclareLaunchArgument(
        "wheel_radius",
        default_value="0.033"
    )

    wheel_separation_arg = DeclareLaunchArgument(
        "wheel_separation",
        default_value="0.17"
    )

    use_python = LaunchConfiguration("use_python")
    wheel_radius = LaunchConfiguration("wheel_radius")
    wheel_separation = LaunchConfiguration("wheel_separation")

    # simple_controller_py = Node(
    #     package="bumperbot_controller",
    #     executable="simple_controller.py",
    #     parameters=[{"wheel_radius": wheel_radius,
    #                  "wheel_separation": wheel_separation
    #                 }],
    #     condition=IfCondition(use_python)
    # )

    # simple_controller_cpp = Node(
    #     package="bumperbot_controller",
    #     executable="simple_controller",
    #     parameters=[{"wheel_radius": wheel_radius,
    #                  "wheel_separation": wheel_separation
    #                 }],
    #     condition=UnlessCondition(use_python)
    # )

    # topic_list = ExecuteProcess(
    #         cmd=["ros2", "topic", "list"],
    #         output="both"
    # )
    
    # gui = ExecuteProcess(
    #     cmd=[
    #         "ros2", "launch", "urdf_tutorial", "display.launch.py",
    #         "model:=/home/covirtue/ROS2-Navigation/src/bumperbot_description/urdf/bumperbot.urdf.xacro"
    #     ],
    #     output="both"
    # )

    group_action = GroupAction([
        Node(
            package="bumperbot_controller",
            executable="simple_controller.py",
            parameters=[{"wheel_radius": wheel_radius,
                        "wheel_separation": wheel_separation
                        }],
            condition=IfCondition(use_python)
        ),
        Node(
            package="bumperbot_controller",
            executable="simple_controller",
            parameters=[{"wheel_radius": wheel_radius,
                        "wheel_separation": wheel_separation
                        }],
            condition=UnlessCondition(use_python)
        ),
        ExecuteProcess(
            cmd=["ros2", "topic", "list"],
            output="both"
        ),
        ExecuteProcess(
            cmd=[
                "ros2", "launch", "urdf_tutorial", "display.launch.py",
                "model:=/home/covirtue/ROS2-Navigation/src/bumperbot_description/urdf/bumperbot.urdf.xacro"
            ],
            output="both"
        )
    ])
 
    return LaunchDescription([
        use_python_arg,
        wheel_radius_arg,
        wheel_separation_arg,
        # simple_controller_py,
        # simple_controller_cpp,
        # topic_list,
        # gui,
        group_action
    ])